
import socket
import json
import copy
import time

import yaml
import os
import traceback
import sys
import numpy as np

import cv2
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Bool
from acupoint_msgs.msg import AcupointImagePixel

# import sys
# sys.path.remove('/opt/ros/melodic/lib/python2.7/dist-packages')


from cv_bridge import CvBridge, CvBridgeError


M=np.matrix([[0,    -1, 0,  -9.5],
             [1,    0,  0,  -408.5],
             [0,    0,  1,  0],
             [0,    0,  0,  1]])
inv_M=np.matrix([[0,    1,  0,   408.5],
                 [-1,   0,  0,   -9.5],
                 [0,    0,  1,   0],
                 [0,    0,  0,   1]])

class CameraDisplay:
    def __init__(self):
        self.actions=self.loadYaml('./exp.yaml')
        if self.actions is None:
            print("Error, load yaml file failed")
            return
       
        # print(self.agv_postions)
         
        self.actions=self.actions['actions']
        self.deg2rad=3.1415926/180
        self.origin_image=None
        self.bridge=CvBridge()
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu_filter_image_pixel", AcupointImagePixel, self.acupointCallback) 
        self.img_sub = rospy.Subscriber("/k4a/color/color", Image,self.imgCallback,queue_size=1)
        self.jaka_arrive=rospy.Subscriber("/jaka_arrive",Bool,self.jakaCallback)
        self.x_lists=[]
        self.y_lists=[]
        self.jaka_arrive=False
        self.acupoint_name=None
        while self.origin_image is None or self.acupoint_name is None:
            rospy.sleep(0.1)
        
        # self.client=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        
        try:
            # self.client.connect(('10.161.229.76',56789))
            index=0
            massage_actions=[]
            
            try:
                temp=[]
                for action in self.actions:
                    method=action['method']
                    if method==-1:
                        massage_actions.append(temp)
                        temp=[]
                    else:
                        temp.append(action)
                for massage_action in massage_actions:
                    while not self.jaka_arrive and not rospy.is_shutdown():
                        self.x_lists=[]
                        self.y_lists=[]
                        for action in massage_action:
                            index=self.acupoint_name.index(action['name'])
                            x=self.acupoint_pixels[index].x
                            y=self.acupoint_pixels[index].y
                            self.x_lists.append(x)
                            self.y_lists.append(y)
                        print(self.x_lists)
                        for i in range(len(self.x_lists)):
                            y_plot = int(self.x_lists[i])
                            x_plot = int(self.y_lists[i])
                            cv2.circle(self.origin_image, (x_plot, y_plot), 20, (0, 255, 255), thickness=-1)
                        cv2.namedWindow("camera_display", 0)
                        cv2.imshow("camera_display", self.origin_image)
                        cv2.waitKey(100)
                    self.jaka_arrive=False
                        
               
            except:
                print("error during actions")
                print(traceback.format_exc())
        except:
            print("connect failed")
            print(traceback.format_exc())
        finally:
            pass
            # self.client.close()
    
    def jakaCallback(self,msg):
        self.jaka_arrive=msg.data   
         
    def imgCallback(self,image_msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
            self.origin_image = cv_image
            
        
        except CvBridgeError as e:
            print(e)
    
  
            
      
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_pixels=copy.deepcopy(msg).pixels
     
    

        
    def loadYaml(self,file_path):
        if not os.path.exists(file_path):
            print("Error, %s not exists"%file_path)
            return
        yaml_data=None
        try:
            with open(file_path) as f:
                yaml_data=yaml.safe_load(f.read())
        except:
            print("Some error occured,the exception message is:")
            print(traceback.format_exc())
        return yaml_data


if __name__ == "__main__":
    rospy.init_node('camera_display_node')
    cd=CameraDisplay()



    

    
